#include "../include/Carte.h"

Carte::Carte(std::vector<Batiment*> *b,std::vector<Entite*> *enti/*, std::vector<Element> el*/,Taille t)
{
	batiments = b;
	entites = enti;
	//elements = el;
	pas = PAS_DU_CHEMIN/10;
	taille = t;
}


std::vector<Position> Carte::coinsDeBlocSurSegment( Bloc b, Position p)
{
	std::vector<Position> resultat;
	Position coin1,coin2;
	float marge = 0.01;
	if( b.getPosition()->x - marge  < p.x && p.x < b.getPosition()->x + marge )
	{
		coin1.x = b.getPosition()->x; 
		coin2.x = b.getPosition()->x;
		coin1.y = b.getPosition()->y + b.getTaille().y; 
		coin2.y = b.getPosition()->y;
	}
	else if( b.getPosition()->x + b.getTaille().x- marge  < p.x && p.x < b.getPosition()->x + b.getTaille().x + marge )
	{
		coin1.x = b.getPosition()->x + b.getTaille().x ; 
		coin2.x = b.getPosition()->x + b.getTaille().x ;
		coin1.y = b.getPosition()->y + b.getTaille().y; 
		coin2.y = b.getPosition()->y;
	}
	else if( b.getPosition()->y - marge  < p.y && p.y < b.getPosition()->y + marge )
	{
		coin1.x = b.getPosition()->x; 
		coin2.x = b.getPosition()->x + b.getTaille().x ;
		coin1.y = b.getPosition()->y;
		coin2.y = b.getPosition()->y;
	}
	else if( b.getPosition()->y + b.getTaille().y - marge  < p.y && p.y < b.getPosition()->y + b.getTaille().y + marge )
	{
		coin1.x = b.getPosition()->x; 
		coin2.x = b.getPosition()->x + b.getTaille().x ;
		coin1.y = b.getPosition()->y + b.getTaille().y; 
		coin2.y = b.getPosition()->y + b.getTaille().y; 
	}
	else
	{
		std::cout<<"[DETECTION COINS][ERREUR IMPORTANTE] La position demandee n'est pas sur un bloc"<<std::endl;
	}

	
	resultat.push_back( coin1);
	resultat.push_back( coin2);
	return resultat;
}

float Carte::getCoefficient( Position src, Position dest)
{
	return ((dest.y - src.y)/(dest.x - src.x));
}
bool Carte::estAligneX(Bloc a, Bloc b)
{
	// a est entre le debut inclus et la fin exclue de b
	if(  b.getPosition()->x <= a.getPosition()->x && a.getPosition()->x < b.getPosition()->x + b.getTaille().x )
		return true;
	// b est entre le debut inclus et la fin exclue de a
	if(  a.getPosition()->x <= b.getPosition()->x && b.getPosition()->x < a.getPosition()->x + a.getTaille().x )
		return true;
	
	return false;
}

bool Carte::estAligneY(Bloc a, Bloc b)
{
	// a est entre le debut inclus et la fin exclue de b
	if(  b.getPosition()->y <= a.getPosition()->y && a.getPosition()->y < b.getPosition()->y + b.getTaille().y )
		return true;
	// b est entre le debut inclus et la fin exclue de a
	if(  a.getPosition()->y <= b.getPosition()->y && b.getPosition()->y < a.getPosition()->y + a.getTaille().y )
		return true;
	
	return false;
}

bool Carte::collision( Bloc a, Bloc b)
{
	return ( estAligneY(a,b) && estAligneX(a,b));
}

Position Carte::intersection(Position a1, Position a2, Position b1, Position b2)
{
	/*
	* y = ax + b
	* -> calcul de a (coeff) et b(offest)
	*/
		

	float coeff1 = ( a1.y - a2.y ) / ( a1.x - a2.x );
	float offset1 = a1.y - (a2.x * coeff1);

	float coeff2 = ( b1.y - b2.y ) / ( b1.x - b2.x );
	float offset2 = b1.y - (b2.x * coeff2);

	// intersection en abcisse
	float x = ( offset1 - offset2 ) / ( coeff2 - coeff1 ) ;

	// intersection en ordonnée
	float y = coeff1 * x + offset1;
	
	Position p;
	p.x = x;
	p.y = y;

	return p;
}	

bool Carte::estPositionDansBloc( Position p, Bloc b, bool bordsInclus=true)
{
	float marge = 0.01;
	bool result = false;	
    if( !bordsInclus ){
	    if(  p.x  >  b.getPosition()->x && p.x < (b.getPosition()->x + b.getTaille().x) )
		    if( p.y  > b.getPosition()->y && p.y < (b.getPosition()->y + b.getTaille().y) )
			    result = true;
    }
    else{
        if(  p.x + marge >=  b.getPosition()->x && p.x - marge <= (b.getPosition()->x + b.getTaille().x) ){
		    if( p.y + marge  >= b.getPosition()->y && p.y - marge <= (b.getPosition()->y + b.getTaille().y) )
			    result = true;
        }
    }
	return result;
}

bool Carte::estPositionDansSegment( Position src, Position dest, Position p)
{
	bool res = false;
	Taille t;

	if( src.x > dest.x )
	{
		float tmp=src.x;
		src.x= dest.x;
		dest.x = tmp;
	}

	if( src.y > dest.y )
	{
		float tmp=src.y;
		src.y= dest.y;
		dest.y = tmp;
	}
	
	t.x = dest.x - src.x;
	t.y = dest.y - src.y;

	Bloc b(src, t);

	
	return estPositionDansBloc( p, b);
}

bool Carte::estProche(Bloc a, Position p)
{
	float max=4.0f;
	float diffx1 = (a.getPosition()->x - p.x)*(a.getPosition()->x - p.x);
	float diffx2 = (a.getPosition()->x+a.getTaille().x - p.x)*(a.getPosition()->x+a.getTaille().x - p.x);
	float diffy1 = (a.getPosition()->y - p.y)*(a.getPosition()->y - p.y);
	float diffy2 = (a.getPosition()->y+a.getTaille().y - p.y)*(a.getPosition()->y+a.getTaille().y - p.y);
	float dist;
	
	dist = sqrt( diffx1 + diffy1 );
	//std::cout<<"Distance : "<<dist<<std::endl;
	if( dist <= max )
		return true;
	
	dist = sqrt( diffx2 + diffy1 );
	//std::cout<<"Distance : "<<dist<<std::endl;
	if( dist <= max )
		return true;	
	
	dist = sqrt( diffx1 + diffy2 );
	//std::cout<<"Distance : "<<dist<<std::endl;
	if( dist <= max )
		return true;

	
	dist = sqrt( diffx2 + diffy2 );
	//std::cout<<"Distance : "<<dist<<std::endl;
	if( dist <= max )
		return true;

	return false;

}
bool Carte::estEmplacementPossible( Bloc b, int existe=1 )
{
	int nbCollisions = 0;

		for( unsigned int i=0; i<batiments->size(); i++)
		{
			if( collision( b, *batiments->at(i)))
				nbCollisions ++;
		}

		for( unsigned int i=0; i<entites->size(); i++)
		{
			if( collision( b, *entites->at(i)))
				nbCollisions ++;
		}
	
	// L'objet est au minimum en collision avec lui meme si il existe deja
	if( nbCollisions < 1 + existe )
		return true;
	else
		return false;
}

Taille Carte::getTaille()
{
	return taille;
}

float Carte::distance( Position a, Position b)
{
	return sqrt(  (b.x - a.x) * (b.x - a.x ) + (b.y - a.y) * (b.y - a.y )  );
}

Position Carte::intersection(Position src, Position dest, Bloc b)
{

/// NEED PROPER WAY TO COMPARE POSITIONS !!!!
    std::vector<Position> positionsValides;
	float x1,x2,y1,y2;
    bool ok;

    // DROITE : y = coeff * x + offset
	float coeff = ( dest.y - src.y ) / ( dest.x - src.x );
	float offset = dest.y - (dest.x * coeff);

	x1 = ( b.getPosition()->y - offset ) / coeff;
	x2 = ( b.getPosition()->y + b.getTaille().y - offset ) / coeff;
	y1 = ( coeff * b.getPosition()->x )+ offset;
	y2 = ( coeff * ( b.getPosition()->x + b.getTaille().x ))+ offset;


	Position p1,p2,p3,p4;
	p1.x = x1;
	p1.y = b.getPosition()->y;
std::cout<<" x: "<<p1.x<<" y1 : "<<p1.y<<std::endl;
	if( estPositionDansSegment( src,dest, p1) )
		if( estPositionDansBloc( p1, b ))
			positionsValides.push_back(p1);

	p2.x = x2;
	p2.y = b.getPosition()->y + b.getTaille().y;
    
    ok = true;
    for( int i=0; i< positionsValides.size(); i++)
    {
        if( Bloc::estMemePositionArrondie( positionsValides[i], p2))
            ok = false;
    }

std::cout<<" x: "<<p2.x<<" y2 : "<<p2.y<<std::endl;
	if( ok && estPositionDansSegment( src,dest, p2) )
		if( estPositionDansBloc( p2, b ))
			positionsValides.push_back(p2);

	p3.y = y1;
	p3.x = b.getPosition()->x;
    ok = true;
    for( int i=0; i< positionsValides.size(); i++)
    {
        if( Bloc::estMemePositionArrondie( positionsValides[i], p3))
            ok = false;
    }
std::cout<<" x: "<<p3.x<<" y3 : "<<p3.y<<std::endl;
	if( ok &&  estPositionDansSegment( src,dest, p3) )
		if( estPositionDansBloc( p3, b ))
			positionsValides.push_back(p3);

	p4.y = y2;
	p4.x = b.getPosition()->x + b.getTaille().x;
    ok = true;
    for( int i=0; i< positionsValides.size(); i++)
    {
        
        if( Bloc::estMemePositionArrondie( positionsValides[i], p4))
            ok = false;
    }
std::cout<<" x: "<<p4.x<<" y4 : "<<p4.y<<std::endl;
	if(  ok && estPositionDansSegment( src,dest, p4) )
		if( estPositionDansBloc( p4, b ))
			positionsValides.push_back(p4);


    std::cout<<"Found "<<positionsValides.size()<<" impact points"<<std::endl;
    for( int i=0; i< positionsValides.size(); i++)
{
    std::cout<<"position is "<< positionsValides[i].x<<" , "<< positionsValides[i].y<<std::endl;
}

    
    Position ret;

    if( positionsValides.size() == 1 )
        ret = positionsValides[0];
    else if( positionsValides.size() > 1 )
        {
            ret = positionsValides[0];
            float distmin = distance( src, positionsValides[0]);
            for( int i=1; i< positionsValides.size(); i++)
            {
                float dist = distance( src, positionsValides[i]);
                if ( dist < distmin )
                {
                    ret = positionsValides[i];
                    distmin = dist;
                }
            }
        }

    if( (positionsValides.size() == 1 && !Bloc::estMemePositionArrondie(src,ret)) || positionsValides.size() > 1 )
        return ret;

    else
    {
	    p1.x =-1;
	    p1.y = -1;
	    return p1;
    }
}

Bloc Carte::premierObstacle( Position src, Position dest,Entite *e, Position *impact)
{
	// 2 facon :
	/*
	*	1 : Je crée un bloc englobant les deux points et je verifie les droites de tous les elements qui sont dans ce bloc
	*	2 : Je traite tous les élements
	*
	*/

	std::vector<Bloc> listeBloc;	
	std::vector<Position> listePoint;
	Bloc obstacle,b;
	Position p;
	float distanceMin;

	// on ignore les entites on ne les considere pas comme un obstacle
	/*for( unsigned int i=0; i< entites->size(); i++ )
	{
		if( e != entites->at(i) )
		{
			//std::cout<<i<<":"<<std::endl;
			p = intersection(src,dest,*(entites->at(i)));
			if( p.x  != -1 || p.y  != -1 )
			{
				std::cout<<"[DETECTED] Entite sur le chemin"<<std::endl;
				listePoint.push_back(p);
				listeBloc.push_back((Bloc)*(entites->at(i)));
			}
		}	
	}*/

	for( unsigned int i=0; i< batiments->size(); i++ )
	{
		p = intersection(src,dest,*(batiments->at(i)));
        // si la position d'impact existe 
		if (p.x  != -1 || p.y  != -1) 
		{
			std::cout<<"[DETECTED] Batiment sur le chemin "<<std::endl;
			listePoint.push_back(p);
			listeBloc.push_back((Bloc)*(batiments->at(i)));
		}
	}


	
	if( listePoint.size() > 0 )
	{
		distanceMin = distance(src,listePoint[0]);
		for( unsigned int i=0; i< listeBloc.size(); i++)
		{
			if( distanceMin >= distance(src,listePoint[i]) )
			{
				distanceMin = distance(src,listePoint[i]);
				obstacle = listeBloc[i];
				impact->x = listePoint[i].x;
				impact->y = listePoint[i].y;
			}
		}



	}
	else
	{ 
// 
	}

	
	return obstacle;
}

Bloc Carte::premierObstacle( Position src, Position dest,Entite *e = NULL /*, Position *impact*/)
{
	// 2 facon :
	/*
	*	1 : Je crée un bloc englobant les deux points et je verifie les droites de tous les elements qui sont dans ce bloc
	*	2 : Je traite tous les élements
	*
	*/

	std::vector<Bloc> listeBloc;	
	std::vector<Position> listePoint;
	Bloc obstacle,b;
	Position p;
	float distanceMin;

	// on ignore les entites on ne les considere pas comme un obstacle
	/*for( unsigned int i=0; i< entites->size(); i++ )
	{
		if( e != entites->at(i) )
		{
			//std::cout<<i<<":"<<std::endl;
			p = intersection(src,dest,*(entites->at(i)));
			if( p.x  != -1 || p.y  != -1 )
			{
				std::cout<<"[DETECTED] Entite sur le chemin"<<std::endl;
				listePoint.push_back(p);
				listeBloc.push_back((Bloc)*(entites->at(i)));
			}
		}	
	}*/

	for( unsigned int i=0; i< batiments->size(); i++ )
	{
		p = intersection(src,dest,*(batiments->at(i)));
    // si il y a une collision
		if( p.x  != -1 || p.y  != -1 )
		{
			//std::cout<<"[DETECTED] Batiment sur le chemin"<<std::endl;
			listePoint.push_back(p);
			listeBloc.push_back((Bloc)*(batiments->at(i)));
		}
	} 

	// selection du point le pluys proche
	if( listePoint.size() > 0 )
	{
		distanceMin = distance(src,listePoint[0]);
		for( unsigned int i=0; i< listeBloc.size(); i++)
		{
			if( distanceMin >= distance(src,listePoint[i]) )
			{
				distanceMin = distance(src,listePoint[i]);
				obstacle = listeBloc[i];
				//impact->x = listePoint[i].x;
				//impact->y = listePoint[i].y;
			}
		}



	}
	else
	{
		//impact->y = -1;		
// 
	}

	
	return obstacle;
}
